#include <gtest/gtest.h>

#include <behaviortree_cpp/bt_factory.h>
#include <fishbot_behavior_tree/shared_resources_node_loader.hpp>
#include <pluginlib/class_loader.hpp>
#include <rclcpp/node.hpp>

#include <fishbot_behavior_tree/fishbot_behavior_tree.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

static const char *xml_text = R"(

 <root BTCPP_format="4" >

     <BehaviorTree ID="MainTree">
        <Sequence name="root">
            <Navigate2Pose action_name="/navigate_to_pose" pose="{pose_msg}"/>
        </Sequence>
     </BehaviorTree>

 </root>
 )";

TEST(BasicBehaviorTests, test_behavior_nav2pose)
{
    pluginlib::ClassLoader<fishbot::behavior_tree::SharedResourcesNodeLoaderBase> class_loader(
        "fishbot_behavior_tree", "fishbot::behavior_tree::SharedResourcesNodeLoaderBase");

    auto node = std::make_shared<rclcpp::Node>("test_node");
    auto shared_resources = std::make_shared<fishbot::behavior_tree::BehaviorContext>(node);

    BT::BehaviorTreeFactory factory;
    {
        auto plugin_instance = class_loader.createSharedInstance("fishbot/navigation_behaviors");
        try
        {
            plugin_instance->registerBehaviors(factory, shared_resources);
        }
        catch (const std::exception &e)
        {
            std::cerr << "Exception caught: " << e.what() << std::endl;
        }
        ASSERT_THROW(plugin_instance->registerBehaviors(factory, shared_resources), std::exception);
    }

    BT::Blackboard::Ptr blackboard = BT::Blackboard::create();
    auto tree = factory.createTreeFromText(xml_text, blackboard);

    geometry_msgs::msg::PoseStamped pose_msg;
    pose_msg.header.frame_id = "map";
    pose_msg.header.stamp = node->get_clock()->now();
    pose_msg.pose.position.x = -4.0;
    pose_msg.pose.position.y = 2.0;
    blackboard->set<geometry_msgs::msg::PoseStamped>("pose_msg", pose_msg);

    tree.tickWhileRunning();

    std::cout << "test finish" << std::endl;
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}